//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include "CDisplay.h"
#include <region.h>
#include <rectangle.h>
#include <CTouch.h>
#include <zenr_api.h>

int g_layer;
EXTERN void SetLCDStateOFF();
EXTERN void SetLCDStateON();
EXTERN Bool32 g_allDevicesDown;
EXTERN ECode NotifyAnimationThreadLDCState(UInt32 state);
EXTERN Boolean IsLcdBlack();
EXTERN CTouch* g_touch;
EXTERN void EnterDeepSleepWithoutLock();
EXTERN void ExitDeepSleepWithoutLock();
EXTERN void CloseSensor();
EXTERN ECode RestorGsensor();

static UInt32 isLCDBeenshut = 0;
extern Int32 g_playThenAllDevicesDown;
UInt32 getisLCDBeenshut() {
    return isLCDBeenshut;
}

#define ColorRGBT666ToRGB565(color) \
    (WORD)((((DWORD)color & 0x3E000) >> 2) | \
    (((DWORD)color & 0x000FC0) >> 1) | \
    (((DWORD)color & 0x00003F) >> 1))

#define ColorRGBT555ToRGB565(color) \
    (WORD)((((WORD)color & 0xFFE0) << 1) | \
    ((WORD)color & 0x001F))

#define ColorRGB565ToRGB888(color) \
    (((color) & 0x0000F800) << 8) \
    | (((color) & 0x000007E0) << 5) \
    | (((color) & 0x0000001F) << 3)

// EXTERN_C void __stdcall CopyBaseplaneToFrameBuffer(
//    /* [in] */ UInt16 * pu32Baseplane,
//    /* [in] */ UInt16 * pu16FrameBuffer,
//   /* [in] */ Int32 n32Size);

INLINE UInt32 PageAround(UInt32 size) {
    return (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
}

extern void Nu_DmaBufFree(physaddr_t ptr, size_t size);
extern void *Nu_DmaBufAlloc(size_t size, Int32 bUserAccess);
static char * s_pDisplayBuf = NULL;

INLINE Void CopyBuffer(
    /* [in] */ PUInt32 pDst,
    /* [in] */ PUInt32 pSrc,
    /* [in] */ UInt32 u32Picth,
    /* [in] */ UInt32 u32DstStride,
    /* [in] */ UInt32 u32SrcStride,
    /* [in] */ UInt32 u32Height)
{
    while (u32Height--) {
        memcpy((PVoid)pDst, (PVoid)pSrc, u32Picth);
        pDst += u32DstStride;
        pSrc += u32SrcStride;
    }
}

class CAutoMutex
{
public:

    CAutoMutex(DzMutex * pMutex)
    {
        WaitResult wr;

        m_pMutex = pMutex;
        m_pMutex->Lock(&wr);
    }

    ~CAutoMutex()
    {
        m_pMutex->Unlock();
    }

    DzMutex * m_pMutex;

private:

    CAutoMutex() {}
};

IDeviceDriver * CDECL CreateDisplay(
    /* [in] */ uint_t deviceNo,
    /* [in] */ void* parameter)
{
    // User can write and read it
    s_pDisplayBuf = (char *)Nu_DmaBufAlloc(240 * 400 * 4, TRUE);
    if (s_pDisplayBuf == NULL) {
        kprintf("Create the DisplayBuf Failed!\n");
    }

    //Init DisplayDriver
    CDisplay* display = new CDisplay;
    if (!display) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Not enough memory\n"));

        return NULL;
    }

    display->AddRef();

    ECode ec = display->constructor();
    if (DRV_FAILED(ec)) {
        display->Release();

        return NULL;
    }

    return (IDeviceDriver *)display;
}

#define SPIN_LOCK

#ifdef SPIN_LOCK
Boolean g_user2frame;
Boolean g_frame2lcd;
#else
DzEvent g_user2frameEvent(FALSE, SIGNALED);
DzEvent g_frame2lcdEvent(FALSE, SIGNALED);
WaitResult wr;
EventState es;
#endif

Void User2FrameComplete()
{
#ifdef SPIN_LOCK
    g_user2frame = FALSE;
#else
    nu_notifyDeviceCallerByHISR((Int32)&g_user2frameEvent, SIGNALED);
#endif
}

Void Frame2LcdComplete()
{
#ifdef SPIN_LOCK
    g_frame2lcd = FALSE;
#else
    nu_notifyDeviceCallerByHISR((Int32)&g_frame2lcdEvent, SIGNALED);
#endif
}

Void WaitForFrame2LCD()
{
    Int32 i = 0;

    while (g_frame2lcd) {
        if (i++ > 50000) {
            kprintf("\n============== WaitForFrame2LCD Failed! \n");
            break;
        }
        DzYield();
    }

    g_user2frame = TRUE;
    g_frame2lcd = TRUE;
}

Void WaitForUser2Frame()
{
    Int32 i = 0;

    while (g_user2frame) {
        if (i++ > 50000) {
            kprintf("\n============== WaitForUser2Frame Failed! \n");
            break;
        }
        DzYield();
    }
}

CDisplay::~CDisplay()
{
    if (m_Overlay) {
        DzFreeDiscontinuousKernelPages((virtaddr_t)m_Overlay);
    }
    if (m_frameBuffer) {
        Nu_DmaBufFree((physaddr_t)m_frameBuffer, m_u32FrameBufferSize);
    }
    if (m_pConvertTable) {
        free(m_pConvertTable);
    }
    if (m_vaOverlay) {
        DzUnmapDiscontinuousKernelPages(
            (virtaddr_t)m_vaOverlay, PageAround(m_u32OverlaySize));
    }
}

//#define DISPLAY_DRIVER_TEST

ECode CDisplay::constructor()
{
    m_u32FrameBufferSize = SCREEN_WIDTH * SCREEN_HEIGHT * 4; // RGB888

    nu_displayShutDown();

    nu_displayInit();

    nu_bspDisplayELcdDMACallbackReg(User2FrameComplete, Frame2LcdComplete);

#ifdef DISPLAY_DRIVER_TEST

#define BUFFER_WIDTH    240
#define BUFFER_HEIGHT   400

    Int32* image = (Int32 *)Nu_DmaBufAlloc(
                    BUFFER_WIDTH * BUFFER_HEIGHT * 4, TRUE);
    assert(image);
    Int32* p = image;

    for (Int32 h = 0; h < BUFFER_HEIGHT; h++) {
        Int32 color;

        if (10 > h % 30 ) {
            color = 0xFF0000FF;
        }
        else if (20 > h % 30) {
            color = 0xFF00FF00;
        }
        else {
            color = 0xFFFF0000;
        }

        for (Int32 w = 0; w < BUFFER_WIDTH; w++) {
            *p++ = color;
        }
    }

    kprintf(">>>>>>>>>>>>>DzGetElapsedTicks = %d\n", DzGetElapsedTicks());

    for (Int32 i = 0; i < 300; i++) {
#ifdef SPIN_LOCK
        while (g_frame2lcd) {
            DzYield();
        }

        g_user2frame = TRUE;
        g_frame2lcd = TRUE;
#else
        g_frame2lcdEvent.Wait(&wr, &es);
#endif

        if (nu_displayLoad((Uint8 *)image,
                0, 0, 240, 400, BUFFER_WIDTH, 0, 0, 0)) {
            DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
        }

#ifdef SPIN_LOCK
        while (g_user2frame) {
            DzYield();
        }
#else
        g_user2frameEvent.Wait(&wr, &es);
#endif
    }

    kprintf(">>>>>>>>>>>>>DzGetElapsedTicks = %d\n", DzGetElapsedTicks());


    memset(image, 0xFF, BUFFER_HEIGHT * BUFFER_WIDTH * 4);

#ifdef SPIN_LOCK
    while (g_frame2lcd) {
        DzYield();
    }

    g_user2frame = TRUE;
    g_frame2lcd = TRUE;
#else
    g_frame2lcdEvent.Wait(&wr, &es);
#endif

    if (nu_displayLoad((Uint8 *)image,
            50, 50, 100, 100, BUFFER_WIDTH, 100, 100, 0)) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
    }

#ifdef SPIN_LOCK
    while (g_user2frame) {
        DzYield();
    }
#else
    g_user2frameEvent.Wait(&wr, &es);
#endif

    DzSleep(0x7FFFFFFF, NULL);
#endif

    m_pConvertTable = (ConvertTable*)malloc(sizeof(ConvertTable));
    if (!m_pConvertTable) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Not enough memory\n"));
        return E_OUT_OF_MEMORY;
    }

    Init_color_table();

    return NOERROR;
}

Void CDisplay::RenderAll(
    /* [in] */ UInt8* address)
{
#ifdef SPIN_LOCK
    WaitForFrame2LCD();
#else
    g_frame2lcdEvent.Wait(&wr, &es);
#endif

    if (m_orientation == ScreenOrientation_Up
            || m_orientation == ScreenOrientation_Down) {
        if (nu_displayLoad(address,
                0, 0, SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_WIDTH,
                0, 0, m_orientation)) {
            DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
#ifdef SPIN_LOCK
            g_user2frame = FALSE;
            g_frame2lcd = FALSE;
#else
            g_user2frameEvent.Notify(SIGNALED);
            g_frame2lcdEvent.Notify(SIGNALED);
#endif
        }
    }
    else {
        if (nu_displayLoad(address,
                0, 0, SCREEN_HEIGHT, SCREEN_WIDTH, SCREEN_HEIGHT,
                0, 0, m_orientation)) {
            DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
#ifdef SPIN_LOCK
            g_user2frame = FALSE;
            g_frame2lcd = FALSE;
#else
            g_user2frameEvent.Notify(SIGNALED);
            g_frame2lcdEvent.Notify(SIGNALED);
#endif
        }
    }

#ifdef SPIN_LOCK
    WaitForUser2Frame();
#else
    g_user2frameEvent.Wait(&wr, &es);
#endif
}

ECode CDisplay::Control(
    /* [in] */ Handle32 controlCode,
    /* [in] */ const MemoryBuf &inBuffer,
    /* [out] */ MemoryBuf* pOutBuffer,
    /* [out] */ IEvent** ppCompletionEvent)
{
    ECode ec = NOERROR;
    CAutoMutex mutex(&m_Mutex);

    //kprintf("*********************************in display Control = controlCode = %d\n", controlCode);

    switch (controlCode) {
        // Init the display hardware
        case 3: {
            ec = InitDisplayHardware(inBuffer, pOutBuffer);
            break;
        }

        // Get baseplane format
        case 7: {
            ec = GetBaseplane(inBuffer, pOutBuffer);
            break;
        }

        // set overlay
        case 8: {
            ec = EnableOverlay(inBuffer, pOutBuffer);
            break;
        }

        // get overlay
        case 9: {
            ec = GetOverlay(inBuffer, pOutBuffer);
            break;
        }

        // shut down
        case 10: {
            ec = DisableOverlay(inBuffer, pOutBuffer);
            break;
        }

        // Render
        case 11: {
            ec = Render(inBuffer, pOutBuffer);
            break;
        }

        // Render overlay
        case 12: {
            ec = RenderOverlay(inBuffer, pOutBuffer);
            break;
        }

        case 13: {
            int temp;

            if (inBuffer.GetPayload() == NULL)
                return E_INVALID_ARGUMENT;
            else
                temp = *(int *)inBuffer.GetPayload();

            if (temp < -1 || temp > 5) {
                return E_INVALID_ARGUMENT;
            }

            g_layer = temp;

            if (-1 != g_layer) {
                g_layer++;
            }

            zapi_HMI->LcdBacklight_Level((UInt8)g_layer);
            break;
        }

        // set alpha
        case 14: {
            ec = SetLayerTransparence(inBuffer, pOutBuffer);
            break;
        }

        // Set Orientation
        case 15: {
            ec = SetOrientation(inBuffer, pOutBuffer);
            break;
        }

        // Set TEMode
        case 16: {
            ec = SetTEModeEnabled(inBuffer, pOutBuffer);
            break;
        }

        case 111:
            // Turn on the backlight.
            if (IsLcdBlack()) {
                DzSyncSystemTime();
                zapi_System->LCDWakeup();
                ExitDeepSleepWithoutLock();
                if (g_allDevicesDown)
                {
                    g_allDevicesDown = FALSE;
                    zapi_System->allDevicesPowerOn();
                }
                SetLCDStateON();
                isLCDBeenshut = 0;
                NotifyAnimationThreadLDCState(1);
            }
            g_touch->Control(2001, NULL_MEMORYBUF, NULL, NULL);
            zapi_HMI->LcdBacklight_On();
            break;
        case 112:
            // Turn off the backlight.
            SetLCDStateOFF();
            g_touch->Control(2000, NULL_MEMORYBUF, NULL, NULL);
            NotifyAnimationThreadLDCState(0);
            zapi_HMI->LcdBacklight_Off();
            zapi_System->LCDSleep();
            EnterDeepSleepWithoutLock();
            isLCDBeenshut = 1;
            break;
        case 1000://Power Suspend
            zapi_HMI->LcdBacklight_Level(1);
            break;
        case 1001://Power Resume
            zapi_HMI->LcdBacklight_Level(g_layer);
            break;
        case 1002://Power Wakeup
            kprintf("---- CDisplay::Control ----1002++\n");
//            Display_Resume(&m_disCxt, 1);//Move to Display_Wake()
//            cprintf("Display on. used %d, Delay %d\n", (OSCR0 - lastwake) * 3, (OSCR0 - displaystamp) * 3);
            if (IsLcdBlack()) {
                DzSyncSystemTime();
                ExitDeepSleepWithoutLock();
                zapi_System->LCDWakeup();
                if (g_allDevicesDown)
                {
                    g_playThenAllDevicesDown = 0;
                    g_allDevicesDown = FALSE;
                    zapi_System->allDevicesPowerOn();
                }
                SetLCDStateON();
                isLCDBeenshut = 0;
                NotifyAnimationThreadLDCState(1);
            }
            Control(11, NULL_MEMORYBUF, NULL, NULL);
            RestorGsensor();
            g_touch->Control(2001, NULL_MEMORYBUF, NULL, NULL);
            zapi_HMI->LcdBacklight_On();
            zapi_HMI->LcdBacklight_Level(g_layer);
            kprintf("---- CDisplay::Control ----1002--\n");
            break;
        case 1006:
            SetLCDStateOFF();
            g_touch->Control(2000, NULL_MEMORYBUF, NULL, NULL);
            NotifyAnimationThreadLDCState(0);
            zapi_HMI->LcdBacklight_Off();
            CloseSensor();
            zapi_System->LCDSleep();
            EnterDeepSleepWithoutLock();
            isLCDBeenshut = 1;
            break;
        case 1010://On2Low
            zapi_HMI->LcdBacklight_Level(1);
            // Keep LCD light on when using overlay2.
            // Overlay1 isn't supported now.
            break;
        case 1011://Low2On
            zapi_HMI->LcdBacklight_Level(g_layer);
            break;
        default:
            ec = E_NOT_IMPLEMENTED;
            break;
    }

    return ec;
}

// private function

ECode CDisplay::InitDisplayHardware(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (!outData || outData->IsNull()) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Invalid argument\n"));
        return E_INVALID_ARGUMENT;
    }

    Address* addr = (Address*)outData->GetPayload();
    outData->SetUsed(sizeof(Address));

    *addr = (Address)s_pDisplayBuf;

    return NOERROR;
}

ECode CDisplay::GetBaseplane(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (!outData || outData->IsNull()) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Invalid argument\n"));
        return E_INVALID_ARGUMENT;
    }

    FrameBufferFormat format;

    format.Color = COLOR_RGB24;
    format.Bpp = 32;
    format.Width = SCREEN_WIDTH;
    format.Height = SCREEN_HEIGHT;

    outData->Copy((PByte)&format, sizeof(FrameBufferFormat));

    return NOERROR;
}

ECode CDisplay::EnableOverlay(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (ebbInData.IsNull()) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Invalid argument\n"));
        return E_INVALID_ARGUMENT;
    }

    if (m_frameBuffer) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Overlay has already opened\n"));
        return E_ALREADY_EXIST;
    }

    OverlayFormat * pOverlayFormat = (OverlayFormat*)ebbInData.GetPayload();

    memcpy((PVoid)&m_OverlayFormat, pOverlayFormat, sizeof(OverlayFormat));

    m_u32OverlaySize = SCREEN_WIDTH * SCREEN_HEIGHT * (pOverlayFormat->Bpp >> 3);

    m_Overlay = (PUInt8)DzAllocDiscontinuousKernelPages(
        PageAround(m_u32OverlaySize), 0);
    if (!m_Overlay) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Not enough memory\n"));
        return E_OUT_OF_MEMORY;
    }

    memset((PVoid)m_Overlay, 0, m_u32OverlaySize);

    m_frameBuffer = (PUInt8)Nu_DmaBufAlloc(m_u32FrameBufferSize, FALSE);
    if (!m_frameBuffer) {
        DzFreeDiscontinuousKernelPages((virtaddr_t)m_Overlay);
        m_Overlay = NULL;
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Not enough memory\n"));
        return E_OUT_OF_MEMORY;
    }

    m_rtOverlay.x = m_OverlayFormat.PosX;
    m_rtOverlay.y = m_OverlayFormat.PosY;
    m_rtOverlay.width = m_OverlayFormat.Width;
    m_rtOverlay.height = m_OverlayFormat.Height;
    m_bRendered = FALSE;

    return GetOverlayOut(2, outData);
}

ECode CDisplay::GetOverlay(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (!outData || outData->IsNull()) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Invalid argument\n"));
        return E_INVALID_ARGUMENT;
    }

    if (!m_frameBuffer) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("overlay is close\n"));
        return E_DOES_NOT_EXIST;
    }

    // BUGBUG: we should invoke DzIoUnmap before disable overlay
    return GetOverlayOut(2, outData);
}

ECode CDisplay::DisableOverlay(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    nu_displayShutDown();

    if (m_frameBuffer) {
        Nu_DmaBufFree((physaddr_t)m_frameBuffer, m_u32FrameBufferSize);
        m_frameBuffer = NULL;
    }

    if (m_Overlay) {
        DzFreeDiscontinuousKernelPages((virtaddr_t)m_Overlay);
        m_Overlay = NULL;
    }

    if (m_vaOverlay) {
        DzUnmapDiscontinuousKernelPages(
            (virtaddr_t)m_vaOverlay, PageAround(m_u32OverlaySize));
        m_vaOverlay = NULL;
    }

    return NOERROR;
}

ECode CDisplay::Render(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (IsLcdBlack()) {
        return NOERROR;
    }

    PUInt8 pu8TargetBuffer;
    Boolean bCanCombine = (NULL != m_frameBuffer);
    if (bCanCombine) m_bRendered = TRUE;

    if (ebbInData.IsEmpty()) {
        // combine overlay and baseplane
        if (!bCanCombine) {
            pu8TargetBuffer = (PUInt8)s_pDisplayBuf;
        }
        else {
            memcpy(
                (PVoid)m_frameBuffer,
                (PVoid)s_pDisplayBuf,
                SCREEN_WIDTH * SCREEN_HEIGHT * 4); // RGB888

            CombineOverlayToFrameBuffer((PUInt8)s_pDisplayBuf, NULL);
            pu8TargetBuffer = (PUInt8)m_frameBuffer;
        }

        RenderAll(pu8TargetBuffer);
    }
    else {
        RenderFrame* frame = (RenderFrame *)ebbInData.GetPayload();
        Rectangle rtFrame = {
            frame->startX,
            frame->startY,
            frame->srcBuffer.width,
            frame->srcBuffer.height};

        // combine overlay and baseplane
        if (!bCanCombine) {
            pu8TargetBuffer = (PUInt8)frame->srcBuffer.address;
        }
        else {
            Int32 n32DstOffset = rtFrame.x + rtFrame.y * SCREEN_WIDTH;
            PUInt32 pu32Src = (PUInt32)frame->srcBuffer.address + n32DstOffset;
            PUInt32 pu32Dst = (PUInt32)m_frameBuffer + n32DstOffset;

            CopyBuffer(
                pu32Dst,
                pu32Src,
                frame->srcBuffer.width * (frame->srcBuffer.bitsPerPixel >> 3),
                SCREEN_WIDTH,
                frame->srcBuffer.pitch >> 2,
                rtFrame.height);

            CombineOverlayToFrameBuffer(
                (PUInt8)s_pDisplayBuf, &rtFrame);

            pu8TargetBuffer = (PUInt8)m_frameBuffer;
        }

        if (m_teMode) {
            RenderAll(pu8TargetBuffer);
            return NOERROR;
        }

#ifdef SPIN_LOCK
        WaitForFrame2LCD();
#else
        g_frame2lcdEvent.Wait(&wr, &es);
#endif
//kprintf("CDisplay parameter: addr = 0x%08x src(%d, %d, %d, %d, %d), dst(%d, %d), m_orientation = %d\n",
//pu8TargetBuffer,
//frame->startX, frame->startY,
//frame->srcBuffer.width, frame->srcBuffer.height,
//frame->srcBuffer.pitch >> 2,
//frame->startX, frame->startY, m_orientation);

       if (!(!m_teMode
            || (frame->startX == 0 && frame->startY == 0
                && frame->srcBuffer.width == 240
                && frame->srcBuffer.height == 400)))
        {
            DRV_EXPRESSION(ERRORLEVEL,
                kprintf("render error %x, %d, %d, %d, %d\n",
                m_teMode, frame->startX, frame->startY,
                frame->srcBuffer.width, frame->srcBuffer.height));
            return E_INVALID_ARGUMENT;
        }

        Int32 nRet = 0;

        if (!bCanCombine) {
            nRet = nu_displayLoad(
                pu8TargetBuffer, frame->startX, frame->startY,
                frame->srcBuffer.width, frame->srcBuffer.height,
                frame->srcBuffer.pitch >> 2,
                frame->startX, frame->startY,
                m_orientation);
        }
        else {
            nRet = nu_displayLoad(
                pu8TargetBuffer, frame->startX, frame->startY,
                frame->srcBuffer.width, frame->srcBuffer.height,
                SCREEN_WIDTH,
                frame->startX, frame->startY,
                m_orientation);
        }

        if (nRet) {
            DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
#ifdef SPIN_LOCK
            g_user2frame = FALSE;
            g_frame2lcd = FALSE;
#else
            g_user2frameEvent.Notify(SIGNALED);
            g_frame2lcdEvent.Notify(SIGNALED);
#endif
        }

#ifdef SPIN_LOCK
        WaitForUser2Frame();
#else
        g_user2frameEvent.Wait(&wr, &es);
#endif
    }

    return NOERROR;
}

ECode CDisplay::RenderOverlay(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (IsLcdBlack()) {
        return NOERROR;
    }

    if (!m_frameBuffer) {
        return E_DOES_NOT_EXIST;
    }

    if ((m_rtOverlay.x < 0)
        || (m_rtOverlay.y < 0)
        || (m_rtOverlay.width <= 0)
        || (m_rtOverlay.height <= 0))
    {
        DRV_EXPRESSION(ERRORLEVEL,
            kprintf("Invalid argument %d, %d, %d, %d\n",
            m_rtOverlay.x, m_rtOverlay.y,
            m_rtOverlay.width, m_rtOverlay.height));
        return NOERROR;
    }

    if (!m_bRendered) {
        memcpy(
            (PVoid)m_frameBuffer,
            (PVoid)s_pDisplayBuf,
            SCREEN_WIDTH * SCREEN_HEIGHT * 4); // RGB888
        m_bRendered = TRUE;
    }

    CombineOverlayToFrameBuffer((PUInt8)s_pDisplayBuf, NULL);

    if (m_teMode) {
        RenderAll(m_frameBuffer);
        return NOERROR;
    }

#ifdef SPIN_LOCK
    WaitForFrame2LCD();
#else
    g_frame2lcdEvent.Wait(&wr, &es);
#endif

    Int32 nRet = nu_displayLoad(
        m_frameBuffer, m_rtOverlay.x, m_rtOverlay.y,
        m_rtOverlay.width, m_rtOverlay.height,
        SCREEN_WIDTH,
        m_rtOverlay.x, m_rtOverlay.y,
        m_orientation);

    if (nRet) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Load data failed\n"));
#ifdef SPIN_LOCK
        g_user2frame = FALSE;
        g_frame2lcd = FALSE;
#else
        g_user2frameEvent.Notify(SIGNALED);
        g_frame2lcdEvent.Notify(SIGNALED);
#endif
    }

#ifdef SPIN_LOCK
    WaitForUser2Frame();
#else
    g_user2frameEvent.Wait(&wr, &es);
#endif

    return NOERROR;
}

ECode CDisplay::SetLayerTransparence(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    Byte byTransLvl;

    if (ebbInData.IsNull())
        byTransLvl = 255; // default
    else
        byTransLvl = *(PByte)ebbInData.GetPayload();

    m_byTransLvl = byTransLvl;

    return NOERROR;
}

ECode CDisplay::SetOrientation(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (ebbInData.IsNullOrEmpty()) {
        if (nu_bspDisplaySetRotationDegree(0)) {
            DRV_EXPRESSION(ERRORLEVEL, kprintf("Set rotation failed\n"));
        }

        m_orientation = ScreenOrientation_Up;
    }
    else {
        Int32 orientation = *(int *)ebbInData.GetPayload();
        if (orientation == m_orientation) {
            return NOERROR;
        }

        assert(!m_teMode);

        if (nu_bspDisplaySetRotationDegree(orientation)) {
            DRV_EXPRESSION(ERRORLEVEL,
                kprintf("Set rotation failed, orientation = %d\n",
                orientation));
            return NOERROR;
        }

        m_orientation = orientation;
    }

    return NOERROR;
}

ECode CDisplay::SetTEModeEnabled(
    /* [in] */ const MemoryBuf &ebbInData,
    /* [out] */ MemoryBuf* outData)
{
    if (ebbInData.IsNullOrEmpty()) {
        DRV_EXPRESSION(ERRORLEVEL, kprintf("Invalid argument\n"));
        return E_INVALID_ARGUMENT;
    }

    Boolean teMode = *(PBoolean)ebbInData.GetPayload();
    if (m_teMode == teMode) {
        return NOERROR;
    }

#ifdef SPIN_LOCK
    assert(!g_user2frame);

    while (g_frame2lcd) {
        DzYield();
    }
#else
    g_frame2lcdEvent.Wait(&wr, &es);
#endif

    if (teMode) {
        nu_bspDisplayEnableTeModeAndSetRotate(m_orientation);
    }
    else {
        nu_bspDisplayDisableTeMode();
    }

    m_teMode = teMode;

    return NOERROR;
}

ECode CDisplay::GetOverlayOut(
    /* [in] */ Int32 n32Layer,
    /* [in] */ MemoryBuf * outData)
{
    if (NULL == outData) {
        return E_INVALID_ARGUMENT;
    }

    if (outData->GetCapacity() < (Int32)sizeof(OverlayFormat)) {
        return E_BUFFER_TOO_SMALL;
    }

    Address aVirtualAddr;
    OverlayFormat * pOverlayFormat = (OverlayFormat*)outData->GetPayload();

    aVirtualAddr = (Address)DzRemapDiscontinuousKernelPages(
        (virtaddr_t)m_Overlay,
        PageAround(m_u32OverlaySize),
        MemoryProtection_RW);
    if (!aVirtualAddr) {
        return E_NOT_ENOUGH_ADDRESS_SPACE;
    }

    outData->SetUsed(sizeof(OverlayFormat));

    pOverlayFormat->Color = m_OverlayFormat.Color;
    pOverlayFormat->Bpp = m_OverlayFormat.Bpp;
    pOverlayFormat->Width = SCREEN_WIDTH;
    pOverlayFormat->Height = SCREEN_HEIGHT;
    pOverlayFormat->Enable = TRUE;
    pOverlayFormat->PosX = 0;
    pOverlayFormat->PosY = 0;
    pOverlayFormat->Layer = 2;
    pOverlayFormat->Flag = 0;

    pOverlayFormat->ChannelSize[0] = SCREEN_WIDTH * SCREEN_HEIGHT;      // Y
    pOverlayFormat->ChannelSize[1] = SCREEN_WIDTH * SCREEN_HEIGHT / 2;  // Cb
    pOverlayFormat->ChannelSize[2] = SCREEN_WIDTH * SCREEN_HEIGHT / 2;  // Cr

    pOverlayFormat->pChannelFb[0] = (Void*)aVirtualAddr; // Y
    pOverlayFormat->pChannelFb[1] = (Void*)((UInt32)pOverlayFormat->pChannelFb[0] + pOverlayFormat->ChannelSize[0]); // Cb
    pOverlayFormat->pChannelFb[2] = (Void*)((UInt32)pOverlayFormat->pChannelFb[1] + pOverlayFormat->ChannelSize[1]); // Cr

    if (!m_vaOverlay) {
        m_vaOverlay = aVirtualAddr;
    }

    return NOERROR;
}

Void CDisplay::CombineOverlay_ARGB(
    /* [in] */ PUInt8 pu8SrcBuffer,
    /* [in] */ Rectangle * prtOverlay)
{
    Int32 n32Offset = prtOverlay->x + prtOverlay->y * SCREEN_WIDTH;
    Int32 w = prtOverlay->width, h = prtOverlay->height;
    Int32 n32Pitch = SCREEN_WIDTH - prtOverlay->width;

    PUInt32 pu32Src = (PUInt32)((PUInt32)pu8SrcBuffer + n32Offset);
    PUInt32 pu32Dst = (PUInt32)((PUInt32)m_frameBuffer + n32Offset);
    PUInt32 pu32Ovl = (PUInt32)((PUInt32)m_Overlay + n32Offset);

    UInt32 u32Baseplane;

    if (w & (0x4 -1)) goto L_Normal;

    while (h--)
    {
        while (w)
        {
            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = *pu32Ovl;
            }

            pu32Dst++;
            pu32Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = *pu32Ovl;
            }

            pu32Dst++;
            pu32Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = *pu32Ovl;
            }

            pu32Dst++;
            pu32Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = *pu32Ovl;
            }

            pu32Dst++;
            pu32Ovl++;

            w -= 4;
        }

        w = prtOverlay->width;
        pu32Src += n32Pitch;
        pu32Dst += n32Pitch;
        pu32Ovl += n32Pitch;
    }

    return;

L_Normal:

    while (h--)
    {
        while (w--)
        {
            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = *pu32Ovl;
            }

            pu32Dst++;
            pu32Ovl++;
        }

        w = prtOverlay->width;
        pu32Src += n32Pitch;
        pu32Dst += n32Pitch;
        pu32Ovl += n32Pitch;
    }
}

Void CDisplay::CombineOverlay_RGB565(
    /* [in] */ PUInt8 pu8SrcBuffer,
    /* [in] */ Rectangle * prtOverlay)
{
    Int32 n32Offset = prtOverlay->x + prtOverlay->y * SCREEN_WIDTH;
    Int32 w = prtOverlay->width, h = prtOverlay->height;
    Int32 n32Pitch = SCREEN_WIDTH - prtOverlay->width;

    PUInt32 pu32Src = (PUInt32)((PUInt32)pu8SrcBuffer + n32Offset);
    PUInt32 pu32Dst = (PUInt32)((PUInt32)m_frameBuffer + n32Offset);
    PUInt16 pu16Ovl = (PUInt16)((PUInt16)m_Overlay + n32Offset);

    UInt32 u32Baseplane;

    if (w & (0x4 -1)) goto L_Normal;

    while (h--)
    {
        while (w)
        {
            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = ColorRGB565ToRGB888(*pu16Ovl);
            }

            pu32Dst++;
            pu16Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = ColorRGB565ToRGB888(*pu16Ovl);
            }

            pu32Dst++;
            pu16Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = ColorRGB565ToRGB888(*pu16Ovl);
            }

            pu32Dst++;
            pu16Ovl++;

            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = ColorRGB565ToRGB888(*pu16Ovl);
            }

            pu32Dst++;
            pu16Ovl++;

            w -= 4;
        }

        w = prtOverlay->width;
        pu32Src += n32Pitch;
        pu32Dst += n32Pitch;
        pu16Ovl += n32Pitch;
    }

    return;

L_Normal:

    while (h--)
    {
        while (w--)
        {
            u32Baseplane = *pu32Src++;
            if (0x00000000 == u32Baseplane) {
                *pu32Dst = ColorRGB565ToRGB888(*pu16Ovl);
            }

            pu32Dst++;
            pu16Ovl++;
        }

        w = prtOverlay->width;
        pu32Src += n32Pitch;
        pu32Dst += n32Pitch;
        pu16Ovl += n32Pitch;
    }
}

Void CDisplay::CombineOverlay_YUYV(
    /* [in] */ PUInt8 pu8SrcBuffer,
    /* [in] */ Rectangle * prtOverlay)
{
    Int32 w = prtOverlay->width, h = prtOverlay->height;
    Int32 n32Offset = prtOverlay->x + prtOverlay->y * SCREEN_WIDTH;
    Int32 n32Pitch = SCREEN_WIDTH - prtOverlay->width;

    UInt32 * pu32Base = (UInt32*)((UInt32*)pu8SrcBuffer + n32Offset);
    UInt32 * pu32Dst = (UInt32*)((UInt32*)m_frameBuffer + n32Offset);
    UInt16 * pu16Ovl = (UInt16*)((UInt16*)m_Overlay + n32Offset);
    UInt8 * pu8Ovl = (UInt8*)pu16Ovl;

    UInt32 u32BaseplanePixel;
    Int32 Y0, Y1, Cb, Cr, R, G, B;

    while (h--)
    {
        while (w--)
        {
            u32BaseplanePixel = *pu32Base;
            if (!(u32BaseplanePixel & 0xFF000000)) {
                if (!(u32BaseplanePixel & 0x00FFFFFF)) {
                    // A  RGB
                    // 0  000
                    Y0 = (*pu8Ovl) & 0xFF;
                    Cb = (*pu8Ovl + 1) & 0xFF;
                    Y1 = (*pu8Ovl + 2) & 0xFF;
                    Cr = (*pu8Ovl + 3) & 0xFF;

                    R = color_table[Ym_tableEx[Y0] + Vm_red_tableEx[Cr]];
                    G = color_table[Ym_tableEx[Y0] + Um_green_tableEx[Cb] + Vm_green_tableEx[Cr]];
                    B = color_table[Ym_tableEx[Y0] + Um_blue_tableEx[Cb]];

                    *pu32Dst = ((R & 0xFF) << 16) | ((G & 0xFF) << 8) | (B & 0xFF);
                }
                // else {
                    // A  RGB
                    // 0  xxx
                    // NOT IMPLEMENTED
                // }
            }
            // else {
                // T  666
                // FF xxx
            // }

            pu32Base++;
            pu32Dst++;

            if (w & 0x01) {
                pu8Ovl += 4;
            }
        }

        w = prtOverlay->width;
        pu32Base += n32Pitch;
        pu32Dst += n32Pitch;
        pu16Ovl += n32Pitch;
        pu8Ovl = (UInt8*)pu16Ovl;
    }
}

Void CDisplay::CombineOverlay_YCbCr422(
    /* [in] */ PUInt8 pu8SrcBuffer,
    /* [in] */ Rectangle * prtOverlay)
{
    Int32 w = prtOverlay->width, h = prtOverlay->height;
    Int32 n32Offset = prtOverlay->x + prtOverlay->y * SCREEN_WIDTH;
    Int32 n32Size = SCREEN_WIDTH * SCREEN_HEIGHT;
    Int32 n32Pitch = SCREEN_WIDTH - prtOverlay->width;

    UInt32 * pu32Base = (UInt32*)((UInt32*)pu8SrcBuffer + n32Offset);
    UInt32 * pu32Dst = (UInt32*)((UInt32*)m_frameBuffer + n32Offset);
    UInt8 * pu8OverlayY = (UInt8*)m_Overlay;
    UInt8 * pu8OverlayCb = (UInt8*)((UInt32)pu8OverlayY + n32Size);
    UInt8 * pu8OverlayCr = (UInt8*)((UInt32)pu8OverlayCb + n32Size >> 1);

    UInt32 u32BaseplanePixel;
    Int32 Y, Cb, Cr, R, G, B;

    pu8OverlayY += n32Offset;
    pu8OverlayCb += (n32Offset >> 1);
    pu8OverlayCr += (n32Offset >> 1);
    UInt8 * pu8Y = pu8OverlayY;
    UInt8 * pu8Cb = pu8OverlayCb;
    UInt8 * pu8Cr = pu8OverlayCr;

    while (h--)
    {
        while (w--)
        {
            u32BaseplanePixel = *pu32Base;
            if (!(u32BaseplanePixel & 0xFF000000)) {
                if (!(u32BaseplanePixel & 0x00FFFFFF)) {
                    // A  RGB
                    // 0  000
                    Y = (*pu8Y) & 0xFF;
                    Cb = (*pu8Cb) & 0xFF;
                    Cr = (*pu8Cr) & 0xFF;
                    R = color_table[Ym_tableEx[Y] + Vm_red_tableEx[Cr]];
                    G = color_table[Ym_tableEx[Y] + Um_green_tableEx[Cb] + Vm_green_tableEx[Cr]];
                    B = color_table[Ym_tableEx[Y] + Um_blue_tableEx[Cb]];

                    *pu32Dst = ((R & 0xFF) << 16) | ((G & 0xFF) << 8) | (B & 0xFF);
                }
                // else {
                    // A  RGB
                    // 0  xxx
                    // NOT IMPLEMENTED
                // }
            }
            // else {
                // A  RGB
                // FF XXX
            // }

            pu32Base++;
            pu32Dst++;

            pu8Y++;
            if (w & 0x01) {
                pu8Cb++;
                pu8Cr++;
            }
        }

        w = prtOverlay->width;
        pu32Base += n32Pitch;
        pu32Dst += n32Pitch;
        pu8Y += n32Pitch;
        pu8Cb += (n32Pitch >> 1);
        pu8Cr += (n32Pitch >> 1);
    }
}

Void CDisplay::CombineOverlayToFrameBuffer(
    /* [in] */ PUInt8 pu8SrcBuffer,
    /* [in] */ Rectangle * prtFrame)
{
    Rectangle rtOverlay = m_rtOverlay;
    ColorFormat colorFormat = m_OverlayFormat.Color;

    if (prtFrame && !RectangleIntersect(&rtOverlay, prtFrame)) {
        return;
    }

    switch (colorFormat) {
        case COLOR_ARGB:
            CombineOverlay_ARGB(pu8SrcBuffer, &rtOverlay);
            break;
        case COLOR_RGB565:
            CombineOverlay_RGB565(pu8SrcBuffer, &rtOverlay);
            break;
        case COLOR_YCbCr422:
            CombineOverlay_YCbCr422(pu8SrcBuffer, &rtOverlay);
            break;
        default:
            break;
    };
}

UInt8 CDisplay::border_color(
    /* [in] */ long color)
{
    if (color > 255)
        return 255;
    else if (color < 0)
        return 0;
    else
        return (UInt8)color;
}

// WARNING: do not use float in kernel mode
const int csY_coeff_16 = (int)(1.164383*(1<<16));
const int csU_blue_16  = (int)(2.017232*(1<<16));
const int csU_green_16 = (int)((-0.391762)*(1<<16));
const int csV_green_16 = (int)((-0.812968)*(1<<16));
const int csV_red_16   = (int)(1.596027*(1<<16));

Void CDisplay::Init_color_table()
{
    _color_table = m_pConvertTable->_color_table;
    Ym_tableEx = m_pConvertTable->Ym_tableEx;
    Um_blue_tableEx = m_pConvertTable->Um_blue_tableEx;
    Um_green_tableEx = m_pConvertTable->Um_green_tableEx;
    Vm_green_tableEx = m_pConvertTable->Vm_green_tableEx;
    Vm_red_tableEx = m_pConvertTable->Vm_red_tableEx;

    for (int i=0;i<256*3;++i) {
        _color_table[i]=border_color(i-256);
    }

    color_table = &_color_table[256];

    for (int i=0;i<256;++i)
    {
        Ym_tableEx[i]=(csY_coeff_16 * (i - 16) )>>16;
        Um_blue_tableEx[i]=(csU_blue_16 * (i - 128) )>>16;
        Um_green_tableEx[i]=(csU_green_16 * (i - 128) )>>16;
        Vm_green_tableEx[i]=(csV_green_16 * (i - 128) )>>16;
        Vm_red_tableEx[i]=(csV_red_16 * (i - 128) )>>16;
    }
}
